// Modified from osg/KdTree
#pragma once

#include <vector>
#include "AABB.hpp"

namespace broadphase {

using uint = unsigned int;

struct Node
{
    Node() : first(0), second(0) {}
    Node(int f, int s) : first(f), second(s) {}

    // if Node is leaf, first < 0, second is the count of leafs,
    // the the start index [is -first-1]
    // if Node is not leaf, first is the left child index, 
    // second is right child index
    int first, second;

    // store the total AABB of all leafs
    AABB bb;

    inline bool isLeaf() const { return first < 0; }
};
using NodeList = std::vector<Node>;
using CollideResult = std::pair<uint, uint>;
using CollideResults = std::vector<CollideResult>;

class Tree 
{
public:
    Tree(uint numPerLeaf = 4) : _numPerLeaf(numPerLeaf) {}

    bool build(const std::vector<AABB>& bbs);

    CollideResults query();
    std::vector<uint> query(const AABB&);

protected:

    int addNode(const Node& node)
    {
        int num = static_cast<int>(_nodes.size());
        _nodes.push_back(node);
        return num;
    }

    void computeDivisions(std::vector<unsigned int>& axisStack);

    int divide(AABB& bb, int nodeIndex, unsigned int level, 
        const std::vector<vec3>& centers,
        const std::vector<uint>& AxisStack);

    void crossChildren(uint n, CollideResults&);
    void queryLeaf(uint leaf, CollideResults&);
    void queryNode(uint l, uint r, CollideResults&);

    void crossChildrenSingle(uint n, CollideResults&);
    void queryNodeSingle(uint l, uint r, CollideResults&);

    uint _numPerLeaf;
    AABB              _bb;    // total aabb
    std::vector<AABB> _bbs;   // all bbs
    std::vector<Node> _nodes; // all nodes
    std::vector<uint> _primitives; 
    std::vector<bool> _nodeCrossed;
};

//typedef Tree<1> SingleTree;

}

#include "Tree.inl"
